#include "constraint.h"

Constraint::Constraint(btDynamicsWorld* ownerWorld, btRigidBody& bodyA, btRigidBody& bodyB, btTransform& localA, btTransform& localB, double limitA, double limitB)
{
  for(int i=0; i < getNumberOfOutputs(); i++)
  {
    Inputs.push_back(false); 
  }
	
	m_ownerWorld = ownerWorld;

// 	btTypedConstraint* c_hinge = new btHingeConstraint( bodyA, bodyB, localA, localB );
// 	c_hinge->m_limitSoftness = 0.9f;
// 	c_hinge->m_solveLimit = false;

// 	hinge = static_cast<btHingeConstraint*>(c_hinge);
	hinge = new btHingeConstraint( bodyA, bodyB, localA, localB );
	
// 	hinge->m_setting.m_impulseClamp = 30.f;
// 	hinge->m_setting.m_tau = 0.1f;
	hinge->setLimit( limitA, limitB, 0.9f, 0.3f, 0.0f );
//   hinge->setAngularOnly(true);
	m_ownerWorld->addConstraint( hinge, true );

	// Calculate full and half range of hinge, HACK could be more efficient
		float smallestLimit = limitA;
		if ( limitB < limitA )
			smallestLimit = limitB;

		diffFromZero = 0.0f - smallestLimit ;
		//cerr << "diff" << diffFromZero << endl;

		fullRange = 0.0f;
		if ( limitA < 0.0f ) fullRange += limitA;
		else fullRange += (limitA * -1.0f);
		if ( limitB < 0.0f ) fullRange += limitB;
		else fullRange += (limitB * -1.0f);

	hinge->setDbgDrawSize(5.0f);

  absolutemotorneurons = Settings::Instance()->getCVarPtr("absolutemotorneurons");
  maximumimpulse = Settings::Instance()->getCVarPtr("maximumimpulse");
}

float Constraint::getAngle()
{
	float percentAngle = (hinge->getHingeAngle() + diffFromZero) / fullRange;
	if ( percentAngle > 1.0f )
		percentAngle = 1.0f;
	if ( percentAngle < 0.0f )
		percentAngle = 0.0f;

	return percentAngle;
}

void Constraint::motorate()
{
	unsigned int firing = 0;
  for(int i=0; i < 10; i++)
    if(Inputs[i])
      firing++;
  
	float angle = firing / 10;
  int   f1 = firing;

  firing = 0;
  for(int i=10; i < 20; i++)
    if(Inputs[i])
      firing++;

  // SJFIXME: Still working all this out.
  float force = (float)firing / 10.0f;
  int   f2   = firing;

  if(*absolutemotorneurons)
  {
    hinge->setMotorTarget(angle,f2+1); //angle, dt);
    hinge->enableMotor(true);
    if(*maximumimpulse)
      hinge->setMaxMotorImpulse(*maximumimpulse);
  }
  else
    hinge->enableAngularMotor(true, (f1 + f2 - 10)*1000, *maximumimpulse);
}

Constraint::~Constraint()
{
	m_ownerWorld->removeConstraint( hinge );
	delete hinge;
}

unsigned int Constraint::registerInputs(BrainzArch* ba, unsigned int id)
{
  // Preserve current behavior; one Input per constraint...
  for(int i=0; i < getNumberOfInputs(); i++)
    ba->registerInput(id++);
  return id;
}

unsigned int Constraint::getNumberOfInputs()
{
  return 10;
}

unsigned int Constraint::registerOutputs(BrainzArch* ba, unsigned int id)
{
  // Preserve current behavior, two Outputs per constraint...
  for(int i=0; i < getNumberOfOutputs(); i++)
    ba->registerOutput(id++);
  return id;
}

unsigned int Constraint::getNumberOfOutputs()
{
  return 20;
}

unsigned int Constraint::procInputs(Brainz& ba, unsigned int id)
{
  float angle = getAngle();
  int numtofire = (int)(angle * getNumberOfInputs());
  for(int i=0; i < getNumberOfInputs(); i++)
  {
    if(numtofire-- >= 0) 
      ba.Inputs[id++].output = 1;
    else
      ba.Inputs[id++].output = 0;
  }
  return id;
}

unsigned int Constraint::procOutputs(Brainz& ba, unsigned int id)
{
  for(int i=0; i < Inputs.size(); i++)
  {
    if(ba.Outputs[id++].output != 0)
      Inputs[i] = true;
    else
      Inputs[i] = false;
  }
  return id;
}



